#include "includes.h"

extern control_struct servo,motor[2],anti[2];
track_status gramb_track;
led_status led[4];

/* time variables */
int amp_history;
uint32 amp_time;
int motor_t;

void led_set(void){
    int e;
    led[0].val=gramb_track.intrack;
    led[3].val=!gramb_track.instraight;
    e=(int)((motor[0].duty_out-motor[0].u)+(motor[1].duty_out-motor[1].u));
    if(-LIGHT_MOTOR_E<=e&&e<=LIGHT_MOTOR_E){
        led[1].val=1;
        led[2].val=1;
    }else if(anti[0].duty_out>0){
        led[1].val=0;
        led[2].val=1;
    }else{
        led[1].val=1;
        led[2].val=0;
    }
}

void led_update(void){
    int i;

    for(i=0; i<4; ++i)
        gpio_set(led[i].port, led[i].ch, led[i].val);
}

void control_update(void){
    int i;

    amp_time=(amp_time+1>=AMP_TIME)?0:amp_time+1;
    motor_t=(motor_t+1>=MOTOR_T)?0:motor_t+1;

    if(gramb_track.control_lock)
        return;

    if(amp_time==0)
        amp_history=IN_RANGE(amp_history+1, AMP_HISTORY);

    /* servo pwm output */
    servo.duty_out=(int)(servo.duty_out+SERVO_DUTY_MIDDLE+0.5);
    servo.duty_out=MIN(servo.duty_out, SERVO_DUTY_MAX);
    servo.duty_out=MAX(servo.duty_out, SERVO_DUTY_MIN);

    /* motor pwm output */
    for(i=0; i<2; ++i){
        motor[i].duty_out=(int)(motor[i].duty_out+0.5);
        motor[i].duty_out=MIN(motor[i].duty_out, MOTOR_DUTY_MAX);
        motor[i].duty_out=MAX(motor[i].duty_out, MOTOR_DUTY_MIN);
    }
    
    /* motor anti pwm output */
    for(i=0; i<2; ++i){
        anti[i].duty_out=(int)(anti[i].duty_out+0.5);
        anti[i].duty_out=MIN(anti[i].duty_out, MOTOR_ANTI_MAX);
        anti[i].duty_out=MAX(anti[i].duty_out, MOTOR_ANTI_MIN);
    }

    /* update time variable */
    servo.t=!servo.t;
    for(i=0; i<2; ++i)
        motor[i].t=IN_RANGE(motor[i].t+1, MOTOR_PID_T);

    /* implementation */
    tpm_pwm_duty(TPM1, TPM_CH0, (int)servo.duty_out);
    tpm_pwm_duty(TPM0, TPM_CH0, (int)motor[0].duty_out);
    tpm_pwm_duty(TPM0, TPM_CH3, (int)motor[1].duty_out);
    tpm_pwm_duty(TPM0, TPM_CH1, (int)anti[0].duty_out);
    tpm_pwm_duty(TPM0, TPM_CH2, (int)anti[1].duty_out);
    led_update();
}
